import datetime

from pydantic import conlist
import asyncio
import time

from common.third_party.devmem import DevMem
from common.third_party.save_config import load_const, save_const, save_config
from services.ad936x.api import Ad936xApi
from services.ad936x.schemas.configs import DoubleConfig
from services.automatic_control.schemas.configs import CFOOffsets
from services.control.api import ControlApi
from services.dsp.api import DspApi
from services.dsp.schemas.configs import DspConfig as DspConfig
from services.relays_module.api import RelaysModuleApi
from services.relays_module.schemas.configs import RmConfig
from services.server_integration.api import ServerIntegrationApi
from services.telemetry.api import TelemetryApi
from settings import settings
from common.third_party.log import new_logger
import logging

class CarrierFrequencyAutomaticControl:
    DSP_API = DspApi()
    AD_API = Ad936xApi()
    TELEMETRY_API = TelemetryApi()
    RM_API = RelaysModuleApi()
    CONTROL_API = ControlApi()
    SI_API = ServerIntegrationApi()

    __slots__ = ['cfo_offsets', 'previous_selected_frequency',
                 'link_tx_out_offset_original', 'link_rtx_dl_offset_original', 'reinit_needed',
                 'actual_reference_freq_40mhz_list', 'link_rx_out_offset_original', 'service_rx_offset_original',
                 'service_tx_offset_original', 'link_rtx_ul_offset_original', 'ra_clocks_measured',
                 'dsp_lo', 'pss_found', '_pps_once_found', '_logger_ra_clock']

    def __init__(self):
        self._logger_ra_clock = new_logger('ra_clock', level=logging.ERROR, stdout_handler=False)

        self.cfo_offsets = CFOOffsets()
        self.previous_selected_frequency = 40_000_000
        self.pss_found = False

        self.link_tx_out_offset_original = DspConfig()
        self.link_rtx_dl_offset_original = DspConfig()
        self.link_rx_out_offset_original = DspConfig()
        self.service_rx_offset_original = DspConfig()
        self.service_tx_offset_original = DspConfig()
        self.link_rtx_ul_offset_original = DspConfig()
        self.dsp_lo = list()

        asyncio.create_task(self.reinit_offsets())

        self._pps_once_found = False

        self.actual_reference_freq_40mhz_list = []

        self.ra_clocks_measured: float | None = load_const('ac/ra_clocks_measured3', float, None)

    async def reinit_offsets(self):
        self.link_tx_out_offset_original = await self.DSP_API.get_dsp_config('LINK_TX_OUT_OFFSET')
        self.link_rtx_dl_offset_original = await self.DSP_API.get_dsp_config('LINK_RTX_DL_OFFSET')
        self.link_rx_out_offset_original = await self.DSP_API.get_dsp_config('LINK_RX_OUT_OFFSET')
        self.service_rx_offset_original = await self.DSP_API.get_dsp_config('SERVICE_RX_OFFSET')
        self.service_tx_offset_original = await self.DSP_API.get_dsp_config('SERVICE_TX_OFFSET')
        self.link_rtx_ul_offset_original = await self.DSP_API.get_dsp_config('LINK_RTX_UL_OFFSET')
        self.dsp_lo = await self.DSP_API.get_lo()

    async def find_pilot_by_power(self, ac, index: int, central_freq=None, bandwidth=100_000):
        async def read_status_of_link():
            for _ in range(8):
                await asyncio.sleep(0.001)
                if ac._read_status_of_link(index):
                    return True

        def read_power():
            return ac._read_pilot_power_max_ab(index)

        try:
            if central_freq is None:
                central_freq = self.get_freq_rx(index)
            else:
                self.set_freq_rx(index, central_freq)

            big_step = 240
            big_bandwidth = bandwidth
            little_step = 10
            little_max_steps = 300
            global_power_map = {}

            unfound_break = 4_000 * 2 // big_step
            unfound_counter = None

            founded_freqs = set()
            for big_index in range(big_bandwidth//big_step):
                big_sign = (big_index % 2) * 2 - 1
                big_freq_offset = big_index//2 * big_step * big_sign
                big_freq = central_freq + big_freq_offset
                self.set_freq_rx(index, big_freq)

                if unfound_counter is not None and unfound_counter > unfound_break:
                    break

                if unfound_counter is not None and not await read_status_of_link():
                    unfound_counter += 1
                else:
                    unfound_counter = 0
                    if big_freq in founded_freqs:
                        continue

                    sfreq, efreq = None, None
                    little_index = 0
                    local_power_map = {}
                    for _ in range(little_max_steps):
                        if sfreq is None:
                            little_freq = big_freq - little_index * little_step
                        else:
                            little_freq = big_freq + little_index * little_step

                        self.set_freq_rx(index, little_freq)
                        founded_freqs.add(little_freq)
                        little_index += 1

                        status_of_link = await read_status_of_link()
                        if not status_of_link and sfreq is None:
                            sfreq = little_freq + little_step
                            little_index = 1
                            continue

                        if not status_of_link and sfreq is not None:
                            efreq = little_freq - little_step
                            break

                        local_power_map[little_freq] = read_power()

                    if efreq is None or sfreq is None or abs(efreq - sfreq) < 200:
                        continue

                    max_freq_power = max(local_power_map, key=local_power_map.get)
                    max_power = local_power_map[max_freq_power]
                    freq_bandwidth = efreq - sfreq
                    if max_freq_power in global_power_map:
                        pass
                    else:
                        global_power_map[max_freq_power] = int(max_power) + freq_bandwidth / 1000

                    continue

            max_freq_power = max(global_power_map, key=global_power_map.get)
            self.set_freq_rx(index, max_freq_power)
            return True
        except Exception as e:
            pass

        self.set_freq_rx(index, central_freq)
        return False

    async def carrier_frequency_control(self, freq: float = None, pilot: bool = True):
        try:
            if freq is None:
                read_ra_clocks_measured = None
                read_ra_clocks_measured_pss = await self.find_pss()
                read_ra_clocks_measured_gps = await self.read_ra_clocks_measured()

                if read_ra_clocks_measured_gps is not None:
                    read_ra_clocks_measured = read_ra_clocks_measured_gps
                    save_const('ac/ra_clocks_measured_gps', read_ra_clocks_measured_gps)

                if read_ra_clocks_measured_pss is not None:
                    save_config('tm/last_bs_capture_time', datetime.datetime.now().timestamp())
                    if read_ra_clocks_measured is None:
                        read_ra_clocks_measured = read_ra_clocks_measured_pss
                    self.pss_found = True
                    save_const('ac/ra_clocks_measured_pss', read_ra_clocks_measured_pss)
                else:
                    self.pss_found = False

                if read_ra_clocks_measured is not None:
                    save_const('ac/ra_clocks_measured3', read_ra_clocks_measured)
                    self.ra_clocks_measured = read_ra_clocks_measured
            else:
                read_ra_clocks_measured_gps = await self.read_ra_clocks_measured()
                if read_ra_clocks_measured_gps is not None:
                    save_const('ac/ra_clocks_measured_gps', read_ra_clocks_measured_gps)

                self.ra_clocks_measured = freq

            config = [await self.AD_API.get_config(i) for i in range(settings.AD936X_NUMBER)]
            rm_config = [await self.RM_API.get_configure(i) for i in range(settings.SERVICES_RELAYS_NUMBER)]

            si_config = await self.SI_API.get_full_configuration()
            block_type = await self.CONTROL_API.get_control_state('block_state')

            if await self.DSP_API.get_reinit() or type(self.link_tx_out_offset_original) != DspConfig:
                await self.reinit_offsets()

            correct_result = await self.correct_offset_calculation(
                link_rx_out_offset=self.link_rx_out_offset_original,
                service_rx_offset=self.service_rx_offset_original,
                service_tx_offset=self.service_tx_offset_original,
                link_rtx_ul_offset=self.link_rtx_ul_offset_original,
                link_tx_out_offset=self.link_tx_out_offset_original,
                link_rtx_dl_offset=self.link_rtx_dl_offset_original,
                block_type=block_type,
                rms=si_config.relay_channels[1].rtr_common.rt_rxs_enabled,
                ops=si_config.relay_channels[1].rtr_common.optic_enabled,
                config=config, rm_config=rm_config, pilot=pilot)

            return correct_result

        except Exception as e:
            #print(f'error during cfo correction {e}')
            return False

    async def correct_offset_calculation(self,
                                   service_tx_offset: DspConfig,
                                   service_rx_offset: DspConfig,
                                   link_tx_out_offset: DspConfig,
                                   link_rx_out_offset: DspConfig,
                                   link_rtx_dl_offset: DspConfig,
                                   link_rtx_ul_offset: DspConfig,
                                   block_type,
                                   rms, ops,
                                   config: conlist(DoubleConfig),
                                   rm_config: conlist(RmConfig), pilot: bool = True) -> bool:

        actual_reference_freq_40mhz = self.ra_clocks_measured
        if actual_reference_freq_40mhz is None:
            return False

        ad0_rx_freq = config[0].rx_frequency
        ad1_rx_freq = config[1].rx_frequency
        ad2_rx_freq = config[2].rx_frequency
        ad3_rx_freq = config[3].rx_frequency

        ad0_tx_freq = config[0].tx_frequency
        ad1_tx_freq = config[1].tx_frequency
        ad2_tx_freq = config[2].tx_frequency

        ad0_rx_multiple_coef = ad0_rx_freq / self.previous_selected_frequency
        ad1_rx_multiple_coef = ad1_rx_freq / self.previous_selected_frequency
        ad2_rx_multiple_coef = ad2_rx_freq / self.previous_selected_frequency
        ad3_rx_multiple_coef = ad3_rx_freq / self.previous_selected_frequency

        ad0_tx_multiple_coef = ad0_tx_freq / self.previous_selected_frequency
        ad1_tx_multiple_coef = ad1_tx_freq / self.previous_selected_frequency
        ad2_tx_multiple_coef = ad2_tx_freq / self.previous_selected_frequency

        delta_freq_40mhz = self.previous_selected_frequency - actual_reference_freq_40mhz

        ad0_rx_delta_freq = delta_freq_40mhz * ad0_rx_multiple_coef * 10 ** -6
        ad1_rx_delta_freq = delta_freq_40mhz * ad1_rx_multiple_coef * 10 ** -6
        ad2_rx_delta_freq = delta_freq_40mhz * ad2_rx_multiple_coef * 10 ** -6
        ad3_rx_delta_freq = delta_freq_40mhz * ad3_rx_multiple_coef * 10 ** -6

        ad0_tx_delta_freq = delta_freq_40mhz * ad0_tx_multiple_coef * 10 ** -6
        ad1_tx_delta_freq = delta_freq_40mhz * ad1_tx_multiple_coef * 10 ** -6
        ad2_tx_delta_freq = delta_freq_40mhz * ad2_tx_multiple_coef * 10 ** -6

        f_ref = 40_000_000  # Reference frequency
        f_step = 40_000_000  # Frequency tuning step

        n_div_tx0_round = round((rm_config[0].tx_frequency - ad0_tx_freq) /
                                round(f_ref / round(f_ref / f_step)))
        n_div_rx0_round = round((rm_config[0].rx_frequency - ad0_rx_freq) /
                                round(f_ref / round(f_ref / f_step)))

        n_div_tx1_round = round((rm_config[1].tx_frequency - ad0_tx_freq) /
                                round(f_ref / round(f_ref / f_step)))
        n_div_rx1_round = round((rm_config[1].rx_frequency - ad0_rx_freq) /
                                round(f_ref / round(f_ref / f_step)))

        rm_rx0_delta_freq = delta_freq_40mhz * n_div_rx0_round * 10 ** -6
        rm_tx0_delta_freq = delta_freq_40mhz * n_div_tx0_round * 10 ** -6

        rm_rx1_delta_freq = delta_freq_40mhz * n_div_rx1_round * 10 ** -6
        rm_tx1_delta_freq = delta_freq_40mhz * n_div_tx1_round * 10 ** -6
        # if pilot and not load_const('force_find_pilot', bool, False):
        #     freq_tx_0 = -26_000_000 + (ad0_tx_delta_freq + rm_tx0_delta_freq) * 1_000_000
        #     freq_tx_1 = -26_000_000 + (ad0_tx_delta_freq + rm_tx1_delta_freq) * 1_000_000
        #     freq_rx_0 = -26_000_000 + (ad0_rx_delta_freq + rm_rx0_delta_freq) * 1_000_000
        #     freq_rx_1 = -26_000_000 + (ad0_rx_delta_freq + rm_rx1_delta_freq) * 1_000_000
        #     freq_rx_2 = -26_000_000 + (ad3_rx_delta_freq + rm_rx0_delta_freq) * 1_000_000
        #
        #     self.set_freq_tx(0, freq_tx_0)
        #     self.set_freq_tx(1, freq_tx_1)
        #     self.set_freq_rx(0, freq_rx_0)
        #     self.set_freq_rx(1, freq_rx_1)
        #     self.set_freq_rx(2, freq_rx_2)

        if settings.CARRIER_FREQUENCY_CONTROL:
            any_rm = any(rms)
            if block_type:
                # FSC Downlink Start
                link_tx_out_offset = DspConfig(
                    c=[link_tx_out_offset.c[0] - ad1_rx_delta_freq,
                       link_tx_out_offset.c[1] - ad1_rx_delta_freq,
                       link_tx_out_offset.c[2] - ad2_rx_delta_freq,
                       link_tx_out_offset.c[3] - ad2_rx_delta_freq])

                if any_rm:
                    link_rtx_dl_offset = DspConfig(
                        c=[offset + ad0_tx_delta_freq + rm_tx0_delta_freq for offset in link_rtx_dl_offset.c])
                # FSC Downlink End

                # FSC Uplink Start
                link_rx_out_offset = DspConfig(
                    c=[link_rx_out_offset.c[0] + ad1_tx_delta_freq,
                       link_rx_out_offset.c[1] + ad1_tx_delta_freq,
                       link_rx_out_offset.c[2] + ad2_tx_delta_freq,
                       link_rx_out_offset.c[3] + ad2_tx_delta_freq])

                if rms[0]:
                    service_rx_offset = DspConfig(
                        c=[offset - ad0_rx_delta_freq - rm_rx0_delta_freq for offset in service_rx_offset.c])

                if rms[1]:
                    link_rtx_ul_offset = DspConfig(
                        c=[offset - ad0_rx_delta_freq - rm_rx1_delta_freq for offset in link_rtx_ul_offset.c])
                # FSC Uplink End

            else:
                # FSR Downlink Start
                if rms[0]:
                    link_tx_out_offset = DspConfig(
                        c=[offset - ad0_rx_delta_freq - rm_rx0_delta_freq for offset in link_tx_out_offset.c])

                type_3_and_4_cr = (await self.SI_API.get_full_configuration()).unit_configuration.channel_3_and_4_config.value
                if type_3_and_4_cr == "LTE_GSM":
                    lo_33 = self.dsp_lo[0] + ad1_tx_delta_freq
                    await self.DSP_API.set_lo(value=lo_33, index=0)
                    lo_34 = self.dsp_lo[1] + ad1_tx_delta_freq
                    await self.DSP_API.set_lo(value=lo_34, index=1)
                else:
                    service_tx_offset = DspConfig(
                        c=[service_tx_offset.c[0],
                           service_tx_offset.c[1] + ad1_tx_delta_freq,
                           service_tx_offset.c[2],
                           service_tx_offset.c[3]])

                service_tx_offset = DspConfig(
                    c=[service_tx_offset.c[0] + ad1_tx_delta_freq,
                       service_tx_offset.c[1],
                       service_tx_offset.c[2] + ad2_tx_delta_freq,
                       service_tx_offset.c[3] + ad2_tx_delta_freq])

                if any_rm:
                    link_rtx_dl_offset = DspConfig(
                        c=[offset + ad1_tx_delta_freq + rm_tx1_delta_freq for offset in link_rtx_dl_offset.c])
                # FSR Downlink End

                # FSR Uplink Start
                service_rx_offset = DspConfig(
                    c=[service_rx_offset.c[0] - ad1_rx_delta_freq,
                       service_rx_offset.c[1] - ad1_rx_delta_freq,
                       service_rx_offset.c[2] - ad2_rx_delta_freq,
                       service_rx_offset.c[3] - ad2_rx_delta_freq])

                lo0_3 = self.dsp_lo[5]
                lo0_3 = lo0_3 - ad2_rx_delta_freq
                await self.DSP_API.set_lo(value=lo0_3, index=5)

                lo0_4 = self.dsp_lo[4]
                lo0_4 = lo0_4 - ad2_rx_delta_freq
                await self.DSP_API.set_lo(value=lo0_4, index=4)

                if rms[1]:
                    link_rtx_ul_offset = DspConfig(
                        c=[offset - ad0_rx_delta_freq - rm_rx1_delta_freq for offset in link_rtx_ul_offset.c])

                if rms[0]:
                    link_rx_out_offset = DspConfig(
                        c=[offset + ad0_tx_delta_freq + rm_tx0_delta_freq for offset in link_rx_out_offset.c])
                # FSR Uplink End

            await self.DSP_API.set_dsp_config(name='SERVICE_TX_OFFSET', config=service_tx_offset, do_reinit=False)
            await self.DSP_API.set_dsp_config(name='LINK_RTX_UL_OFFSET', config=link_rtx_ul_offset, do_reinit=False)
            await self.DSP_API.set_dsp_config(name='SERVICE_RX_OFFSET', config=service_rx_offset, do_reinit=False)
            await self.DSP_API.set_dsp_config(name='LINK_RX_OUT_OFFSET', config=link_rx_out_offset, do_reinit=False)
            await self.DSP_API.set_dsp_config(name='LINK_TX_OUT_OFFSET', config=link_tx_out_offset, do_reinit=False)
            await self.DSP_API.set_dsp_config(name='LINK_RTX_DL_OFFSET', config=link_rtx_dl_offset, do_reinit=False)

        return True

    def set_freq_rx(self, index: int, freq=999450.6835937):
        base_address = [0x83E0_0000, 0x83E1_0000, 0x83E5_0000][index]
        freq_code = int((4294967296 * freq) / 60_000_000)
        DevMem(address=base_address + 4 * 12)\
            .write(0, freq_code)

    def set_freq_tx(self, index: int, freq=999450.6835937):
        base_address = [0x83E0_0000, 0x83E1_0000, 0x83E5_0000][index]
        freq_code = int((4294967296 * freq) / 60_000_000)
        DevMem(address=base_address + 4 * 3)\
            .write(0, freq_code)

    def get_freq_rx(self, index: int):
        base_address = [0x83E0_0000, 0x83E1_0000, 0x83E5_0000][index]
        freq_code = DevMem(address=base_address + 4 * 12) \
            .read(0).get_value()

        return freq_code * 60_000_000 / 4294967296 - 60_000_000

    def get_freq_tx(self, index: int):
        base_address = [0x83E0_0000, 0x83E1_0000, 0x83E5_0000][index]
        freq_code = DevMem(address=base_address + 4 * 3) \
            .read(0).get_value()

        return freq_code * 60_000_000 / 4294967296 - 60_000_000

    async def find_pss(self) -> float | None:
        async def reload_pss_core(code: int):
            mem = DevMem(address=0x84A00000, number_of_words=4)
            mem.write(0, 0)
            await asyncio.sleep(1)

            code = (0x00061000, 0x01061000, 0x02061000, 0x03061000)[code]
            mem.write(4, code)
            mem.write(0, 1)
            await asyncio.sleep(1)

            mem.write(0, 3)

        try:
            ra_clocks_measured = self.read_ra_clocks_measured_pss()
            if self._pps_once_found:
                return ra_clocks_measured

            if ra_clocks_measured is not None:
                return ra_clocks_measured

            block_type = await self.CONTROL_API.get_control_state('block_state')
            if block_type:
                sms_state = (True, True)
            else:
                sms_state = (await self.CONTROL_API.get_control_state('service_0_state'), await self.CONTROL_API.get_control_state('service_1_state'))

            word = (0x10, 0x01)[int(block_type)]
            sm_mem = DevMem(address=0x83E60018, number_of_words=1)
            for sm_index in range(2):
                if not sms_state[sm_index]:
                    continue

                sm_mem.write(0, (0x0, word)[sm_index])
                for code in range(3, 0, -1):
                    await reload_pss_core(code)
                    await asyncio.sleep(66)
                    for _ in range(28):
                        await asyncio.sleep(10)

                        ra_clocks_measured = self.read_ra_clocks_measured_pss()
                        if ra_clocks_measured is not None:
                            self._pps_once_found = True
                            return ra_clocks_measured

        except Exception as e:
            pass

        return None

    async def read_ra_clocks_measured(self) -> float | None:
        if not (await self.TELEMETRY_API.get_gps_telemetry()).status:
            return None

        ra_clocks_base_addr = 0x43C1_0000
        mem = DevMem(address=ra_clocks_base_addr, number_of_words=40)

        av0, av1, av2, av3 = mem.read(0x0).get_value(), mem.read(0x10).get_value(), mem.read(0x20).get_value(), mem.read(0x20).get_value()
        ra_clocks_measured = 0
        if av0:
            lo = mem.read(0x8).get_value()
            hi = mem.read(0xC).get_value()
            hi = hi << 32
            ra_clocks_measured = (hi + lo) / 10 - 1
        if av1:
            lo = mem.read(0x18).get_value()
            hi = mem.read(0x1C).get_value()
            hi = hi << 32
            ra_clocks_measured = (hi + lo) / 100 - 1
        if av2:
            lo = mem.read(0x28).get_value()
            hi = mem.read(0x2C).get_value()
            hi = hi << 32
            ra_clocks_measured = (hi + lo) / 500 - 1
        # if av2:
        #     lo = mem.read(0x38).get_value()
        #     hi = mem.read(0x3C).get_value()
        #     hi = hi << 32
        #     ra_clocks_measured = (hi + lo) / 1000 - 1
        else:
            return None

        if not 4e7 - 220 <= ra_clocks_measured <= 4e7 + 220:
            return None

        return ra_clocks_measured

    def read_ra_clocks_measured_pss(self) -> float | None:
        ra_clocks_base_addr = 0x84B0_0000
        mem = DevMem(address=ra_clocks_base_addr, number_of_words=40)

        av0, av1, av2, av3 = mem.read(0x0).get_value(), mem.read(0x10).get_value(), mem.read(0x20).get_value(), mem.read(0x20).get_value()
        ra_clocks_measured = 0
        if av0:
            lo = mem.read(0x8).get_value()
            hi = mem.read(0xC).get_value()
            hi = hi << 32
            ra_clocks_measured = (hi + lo) / 10 - 1
        if av1:
            lo = mem.read(0x18).get_value()
            hi = mem.read(0x1C).get_value()
            hi = hi << 32
            ra_clocks_measured = (hi + lo) / 100 - 1
        if av2:
            lo = mem.read(0x28).get_value()
            hi = mem.read(0x2C).get_value()
            hi = hi << 32
            ra_clocks_measured = (hi + lo) / 500 - 1
        # if av2:
        #     lo = mem.read(0x38).get_value()
        #     hi = mem.read(0x3C).get_value()
        #     hi = hi << 32
        #     ra_clocks_measured = (hi + lo) / 1000 - 1

        if not 4e7 - 220 <= ra_clocks_measured <= 4e7 + 220:
            return None

        return ra_clocks_measured
